/*
 * Defines various structures, routines, etc. for buffering COM1 I/O and
 * scheduling delayed I/O events.
 */

// time in milliseconds since timer 4 started
int timer4_read() {
  int *timer4_lval = (int*)( TIMER4_LVAL );
  return *timer4_lval / TIMER4_TICKS_PER_MS;
}

void uart_report_status(int base) {
  int* rsr = (int*)( base + UART_RSR_OFFSET );
  int* flag = (int*)( base + UART_FLAG_OFFSET );
  int val;
  val = *rsr;
  bwprintf( COM2, "rsr: 0x%x\r\n", val );
  val = *flag;
  bwprintf( COM2, "flags: 0x%x\r\n", val );
}

// stores info on a single sensor module
struct ssmod {
  int id;
  unsigned char b1;
  unsigned char b2;
};

// single I/O event, one of:
// TX - transmit event; (b1, b2) is a command to be sent to the controller
// RX - receive event; (b1, b2) is a buffer for the sensor module response
struct io {
  int data;           // TX: timestamp to send at
                      // RX: index of sensor module
  unsigned char b1;   // first byte to send
  unsigned char b2;   // second byte to send
};

// buffer for I/O events
struct iobuf {
  struct io buf[IO_BUF_SIZE];      // events buffer
  int front;                       // index for front of queue
  int back;                        // index for back of queue
  int size;                        // number of events in buffer
};

// Certain things, like "solenoid off" commands and the various parts of
// the rv shell command, need to be sent after a given delay. We place such
// events in this queue with a timestamp. The polling loop then repeatedly
// queries the queue for the next event, moving it to the transmit buffer
// when the timestamp is reached.
struct txq {
  struct io q[IO_BUF_SIZE];        // events heap
  int size;                        // number of events in buffer
};

// I/O direct and timed queues
static struct iobuf rxbuf, txbuf;  // receive and transmit buffers
static struct txq timeq;           // timed heap for TX events

// sensor info
static struct ssmod ss_last;
static int ss_last_poll;
static int ss_max_response;

void ssmod_init(struct ssmod *ss) {
  ss->id = ss->b1 = ss->b2 = 0;
}

void ssmod_print(struct ssmod *ss) {
  if (!ss->id) {
    bwprintf( COM2, "NULL\r\n" );
    return;
  }
  char group = 'A'+(ss->id)-1;
  unsigned short val = (ss->b1 << 8) | ss->b2;
  int bit = 16;
  while (!(val & 1)) {
    val >>= 1;
    bit--;
  }
  bwprintf( COM2, "%c%d\r\n", group, bit );
}

void iobuf_init(struct iobuf *ib) {
  ib->front = ib->back = ib->size = 0;
}

int iobuf_empty(struct iobuf *ib) {
  return ib->size == 0;
}

int iobuf_size(struct iobuf *ib) {
  return ib->size;
}

int iobuf_enqueue(struct iobuf *ib, struct io i) {
  if (ib->size == IO_BUF_SIZE) return 0;
  ib->buf[ib->back] = i;
  ib->back++; if (ib->back == IO_BUF_SIZE) ib->back = 0;
  ib->size++;
  return 1;
}

int iobuf_dequeue(struct iobuf *ib, struct io* out) {
  if (ib->size == 0) return 0;
  *out = ib->buf[ib->front];
  ib->front++; if (ib->front == IO_BUF_SIZE) ib->front = 0;
  ib->size--;
  return 1;
}

void io_transmit(struct io *i) {
  bwputc( COM1, i->b1 );
  if (i->b1 == TRAIN_SOLENOID_OFF ||
      i->b1 > TRAIN_SW_CURVED) return;
  bwputc( COM1, i->b2 );
}

void io_receive(struct io *i) {
  i->b1 = bwgetc( COM1 );
  i->b2 = bwgetc( COM1 );
}

int txq_empty(struct txq *tq) {
  return tq->size == 0;
}

int txq_size(struct txq *tq) {
  return tq->size;
}

void txq_init(struct txq *tq) {
  tq->size = 0;
}

int txq_top(struct txq *tq, struct io *out) {
  if (tq->size == 0) return 0;
  *out = tq->q[0];
  return 1;
}

int txq_pop(struct txq *tq) {
  if (tq->size == 0) return 0;
  int k = 0, cL, cR, c, s = tq->size/2;
  struct io tmp;
  tq->q[0] = tq->q[--tq->size];
  while (k < s) {
    cL = 2*k+1; cR = cL+1;
    c = cL;
    if (cR < tq->size && ((tq->q[cR]).data < (tq->q[cL]).data)) c = cR;
    if ((tq->q[k]).data < (tq->q[c]).data) break;
    tmp = tq->q[k]; tq->q[k] = tq->q[c]; tq->q[c] = tmp;
    k = c;
  }
  return 1;
}

int txq_push(struct txq *tq, struct io i) {
  if (tq->size == IO_BUF_SIZE) return 0;
  int k = tq->size, p;
  struct io tmp;
  tq->q[tq->size++] = i;
  while (k > 0) {
    p = (k-1)/2;
    if ((tq->q[k]).data >= (tq->q[p]).data) break;
    tmp = tq->q[k]; tq->q[k] = tq->q[p]; tq->q[p] = tmp;
    k = p;
  }
  return 1;
}

int evio_rxready() {
  int *flag = (int*)( UART1_BASE + UART_FLAG_OFFSET );
  return !iobuf_empty(&rxbuf) && ( *flag & RXFF_MASK ); 
}

int evio_txready() {
  int *flag = (int*)( UART1_BASE + UART_FLAG_OFFSET );
  return !iobuf_empty(&txbuf) && !( *flag & RXFF_MASK );
}

int evio_qready() {
  if (txq_empty(&timeq)) return 0;
  int now = timer4_read();
  struct io i;
  txq_top(&timeq, &i);
  return now >= i.data;
}

int ss_needs_poll() {
  int now = timer4_read();
  return now - ss_last_poll >= 1000;
}

void evio_rx() {
  // receive the next RX event
  struct io rx;
  iobuf_dequeue(&rxbuf, &rx);
  io_receive(&rx);
  if (rx.b1 || rx.b2) {
    ss_last.id = rx.data;
    ss_last.b1 = rx.b1; ss_last.b2 = rx.b2;
  }
  int response = timer4_read() - ss_last_poll;
  if (response > ss_max_response) ss_max_response = response;
}

void evio_tx() {
  // transmit the next TX event
  struct io tx;
  iobuf_dequeue(&txbuf, &tx);
  io_transmit(&tx);
}

void evio_q() {
  // move the next TX event unless the TX queue is full
  struct io tx;
  txq_top(&timeq, &tx);
  if (!iobuf_enqueue(&txbuf, tx)) return;
  txq_pop(&timeq);
}

void ss_poll() {
  struct io i;
  i.b1 = TRAIN_SENSORS_BASE + NUM_SENSOR_MODULES; i.b2 = 0x00;
  iobuf_enqueue(&txbuf, i);
  int m;
  for (m = 1; m <= 5; m++) {
    // (eventually) receive the sensor data
    i.data = m;
    i.b1 = 0x00; i.b2 = 0x00;
    iobuf_enqueue(&rxbuf, i);
  }
  ss_last_poll = timer4_read();
}

void evio_init() {
  // init data structures
  iobuf_init(&rxbuf);
  iobuf_init(&txbuf);
  txq_init(&timeq);

  // init sensor info
  ssmod_init(&ss_last);
  ss_last_poll = 0;
  ss_max_response = 0;

  // enable timer 4
  // in blasphemous fashion, we use this to schedule events
  int *timer4_hval = (int*)( TIMER4_HVAL );
  *timer4_hval = T4EN_MASK;
}
